/*
 * safecheck.cpp
 *
 *  Created on: 2023年2月22日
 *      Author: Jiapeng Chen
 */

#include "safecheck/safecheck.hpp"
#include "flow/flow.hpp"
//自动校准icm20602
void SAFECHECK::calibration()
{
	osDelay(100);
	if (isInit==false)
	{
		xQueuePeek(queueGyrDat, &gyro, 0);
		xQueuePeek(queueAccDat, &acc, 0);
		for(int i=0;i<2;i++)
		{
			acc.acc_offset[i] = 0;
			gyro.gyro_offset[i] = 0;
		}
		for (int i=0;i<3;i++)
		{
			acc_cali[cali_count][i] = acc.acc_raw[i];
			gyro_cali[cali_count][i] = gyro.gyro_raw[i];
		}
		cali_count++;
	}

	if (cali_count==cali_count_done)
	{
		for(int j=0;j<cali_count_done;j++)
		{
			for(int k=0;k<3;k++)
			{
				acc.acc_offset[k] += acc_cali[j][k];
				gyro.gyro_offset[k] += gyro_cali[j][k];
			}
		}

		acc.acc_offset[0] = - acc.acc_offset[0] / cali_count_done + acc_mech_off1;
		acc.acc_offset[1] = - acc.acc_offset[1] / cali_count_done + acc_mech_off2;
		acc.acc_offset[2] = - 4096 - acc.acc_offset[2] / cali_count_done;
		gyro.gyro_offset[0] = - gyro.gyro_offset[0] / cali_count_done;
		gyro.gyro_offset[1] = - gyro.gyro_offset[1] / cali_count_done;
		gyro.gyro_offset[2] = - gyro.gyro_offset[2] / cali_count_done;

		xQueueOverwrite(queueGyrDat, &gyro);
		xQueueOverwrite(queueAccDat, &acc);
		isInit = true;
		cali_count = 0;
	}
}
//光流断线检测(仅能检测开机时光流是否在线)
void SAFECHECK::check()
{
	xQueuePeek(queueFlow, &flow, 0);
	xQueuePeek(queueHeight, &height, 0);
	//如果光流掉线
	if(height.height == 0.f && flow.flowx == 0.f && flow.flowy == 0.f)
	{
		//将control任务挂起，亮故障灯
		if(isSuspend == false)
		{
			vTaskSuspend(attitudeCtrlTaskHandle);
			isSuspend = true;

		}
	}
	else
	{
		if (isSuspend == true)
		{
			vTaskResume(attitudeCtrlTaskHandle);
			isSuspend = false;
		}
	}

}

SAFECHECK safecheck;
extern "C" void safecheck_main(void *argument)
{
	for(;;)
	{
		osSemaphoreAcquire(semSafeCheck,0xffffffff);
		safecheck.calibration();
		safecheck.check();
	}
}

/************************ (C) COPYRIGHT Longmen Drone Team *****END OF FILE****/


